/******************************************************************************
 * Copyright 2023 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/canbus_vehicle/x3epro/x3epro_controller.h"

#include <string>

#include "modules/common_msgs/basic_msgs/vehicle_signal.pb.h"

#include "cyber/common/log.h"
#include "cyber/time/time.h"
#include "modules/canbus/vehicle/vehicle_controller.h"
#include "modules/canbus_vehicle/x3epro/x3epro_message_manager.h"
#include "modules/drivers/canbus/can_comm/can_sender.h"
#include "modules/drivers/canbus/can_comm/protocol_data.h"

namespace apollo {
namespace canbus {
namespace x3epro {
using ::apollo::common::ErrorCode;
using ::apollo::common::VehicleSignal;
using ::apollo::control::ControlCommand;
using ::apollo::drivers::canbus::ProtocolData;

namespace {
const int32_t kMaxFailAttempt = 10;
const int32_t CHECK_RESPONSE_STEER_UNIT_FLAG = 1;
const int32_t CHECK_RESPONSE_SPEED_UNIT_FLAG = 2;

}  // namespace

ErrorCode X3eproController::Init(
    const VehicleParameter& params,
    CanSender<::apollo::canbus::X3epro>* const can_sender,
    MessageManager<::apollo::canbus::X3epro>* const message_manager) {
  if (is_initialized_) {
    AINFO << "X3eproController has already been initiated.";
    return ErrorCode::CANBUS_ERROR;
  }

  vehicle_params_.CopyFrom(
      common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param());
  params_.CopyFrom(params);
  if (!params_.has_driving_mode()) {
    AERROR << "Vehicle conf pb not set driving_mode.";
    return ErrorCode::CANBUS_ERROR;
  }

  if (can_sender == nullptr) {
    AERROR << "Canbus sender is null.";
    return ErrorCode::CANBUS_ERROR;
  }
  can_sender_ = can_sender;

  if (message_manager == nullptr) {
    AERROR << "protocol manager is null.";
    return ErrorCode::CANBUS_ERROR;
  }
  message_manager_ = message_manager;

  // sender part
  ads_brake_command_46_ = dynamic_cast<Adsbrakecommand46*>(
      message_manager_->GetMutableProtocolDataById(Adsbrakecommand46::ID));
  if (ads_brake_command_46_ == nullptr) {
    AERROR << "Adsbrakecommand46 does not exist in the X3eproMessageManager!";
    return ErrorCode::CANBUS_ERROR;
  }

  ads_drive_command_50_ = dynamic_cast<Adsdrivecommand50*>(
      message_manager_->GetMutableProtocolDataById(Adsdrivecommand50::ID));
  if (ads_drive_command_50_ == nullptr) {
    AERROR << "Adsdrivecommand50 does not exist in the X3eproMessageManager!";
    return ErrorCode::CANBUS_ERROR;
  }

  ads_eps_command_56_ = dynamic_cast<Adsepscommand56*>(
      message_manager_->GetMutableProtocolDataById(Adsepscommand56::ID));
  if (ads_eps_command_56_ == nullptr) {
    AERROR << "Adsepscommand56 does not exist in the X3eproMessageManager!";
    return ErrorCode::CANBUS_ERROR;
  }

  ads_lampcommand_309_ = dynamic_cast<Adslampcommand309*>(
      message_manager_->GetMutableProtocolDataById(Adslampcommand309::ID));
  if (ads_lampcommand_309_ == nullptr) {
    AERROR << "Adslampcommand309 does not exist in the X3eproMessageManager!";
    return ErrorCode::CANBUS_ERROR;
  }

  security_brake_command_44_ = dynamic_cast<Securitybrakecommand44*>(
      message_manager_->GetMutableProtocolDataById(Securitybrakecommand44::ID));
  if (security_brake_command_44_ == nullptr) {
    AERROR
        << "Securitybrakecommand44 does not exist in the X3eproMessageManager!";
    return ErrorCode::CANBUS_ERROR;
  }

  can_sender_->AddMessage(Adsbrakecommand46::ID, ads_brake_command_46_, false);
  can_sender_->AddMessage(Adsdrivecommand50::ID, ads_drive_command_50_, false);
  can_sender_->AddMessage(Adsepscommand56::ID, ads_eps_command_56_, false);
  can_sender_->AddMessage(Adslampcommand309::ID, ads_lampcommand_309_, false);
  can_sender_->AddMessage(Securitybrakecommand44::ID,
                          security_brake_command_44_, false);

  // need sleep to ensure all messages received
  AINFO << "X3eproController is initialized.";

  is_initialized_ = true;
  return ErrorCode::OK;
}

X3eproController::~X3eproController() {}

bool X3eproController::Start() {
  if (!is_initialized_) {
    AERROR << "X3eproController has NOT been initiated.";
    return false;
  }
  const auto& update_func = [this] { SecurityDogThreadFunc(); };
  thread_.reset(new std::thread(update_func));

  return true;
}

void X3eproController::Stop() {
  if (!is_initialized_) {
    AERROR << "X3eproController stops or starts improperly!";
    return;
  }

  if (thread_ != nullptr && thread_->joinable()) {
    thread_->join();
    thread_.reset();
    AINFO << "X3eproController stopped.";
  }
}

Chassis X3eproController::chassis() {
  chassis_.Clear();

  X3epro chassis_detail;
  message_manager_->GetSensorData(&chassis_detail);

  // 21, 22, previously 1, 2
  // if (driving_mode() == Chassis::EMERGENCY_MODE) {
  //   set_chassis_error_code(Chassis::NO_ERROR);
  // }

  chassis_.set_driving_mode(driving_mode());
  chassis_.set_error_code(chassis_error_code());
  // 3
  chassis_.set_engine_started(true);

  /* ADD YOUR OWN CAR CHASSIS OPERATION
  // 10 battery soc
  // 11 vin
  // 12 bumper event
  */

  if (chassis_detail.has_vcu_vehicle_status_report_200()) {
    // 5 speed_mps
    chassis_.set_speed_mps(std::abs(static_cast<float>(
        chassis_detail.vcu_vehicle_status_report_200().vcu_vehicle_speed())));

    // 21 driving_mode // set emergency mode only, ignore others
    if (chassis_detail.vcu_vehicle_status_report_200().control_mode_resp() >= 4){
      chassis_.set_driving_mode(Chassis::EMERGENCY_MODE);
    }
    
    // 34 battery_soc_percentage
    chassis_.set_battery_soc_percentage(
        chassis_detail.vcu_vehicle_status_report_200().vcu_display_soc());
  } else {
    chassis_.set_speed_mps(0.0);
    chassis_.set_driving_mode(Chassis::EMERGENCY_MODE);
    chassis_.set_battery_soc_percentage(0.0);
  }

  if (chassis_detail.has_vcu_drive_report_52()) {
    // 8 throttle_percentage
    chassis_.set_throttle_percentage(
        chassis_detail.vcu_drive_report_52().vcu_real_torque() / 64 * 100);
    // 23 gear_location
    if (chassis_detail.vcu_drive_report_52().vcu_real_shift() == 3)
      chassis_.set_gear_location(Chassis::GEAR_INVALID);
    else
      chassis_.set_gear_location(
          static_cast<apollo::canbus::Chassis_GearPosition>(
              chassis_detail.vcu_drive_report_52().vcu_real_shift()));
  } else {
    chassis_.set_throttle_percentage(0.0);
    chassis_.set_gear_location(Chassis::GEAR_NONE);
  }

  if (chassis_detail.has_vcu_brake_report_47()) {
    // 9 brake_percentage
    chassis_.set_brake_percentage(
        chassis_detail.vcu_brake_report_47().vcu_real_brake());
    // 13 parking_brake
    if (chassis_detail.vcu_brake_report_47().vcu_real_parking_status() == 1) {
      chassis_.set_parking_brake(true);
    } else {
      chassis_.set_parking_brake(false);
    }
  } else {
    chassis_.set_brake_percentage(0.0);
    chassis_.set_parking_brake(false);
  }

  if (chassis_detail.has_vcu_eps_report_57()) {
    // 11 steering_percentage
    chassis_.set_steering_percentage(-static_cast<float>(
        chassis_detail.vcu_eps_report_57().vcu_real_angle() * 100 /
        vehicle_params_.max_steer_angle() * M_PI / 180));
    // 37 steering_percentage_cmd
    chassis_.set_steering_percentage_cmd(-static_cast<float>(
        chassis_detail.vcu_eps_report_57().vcu_target_angle() * 100 /
        vehicle_params_.max_steer_angle() * M_PI / 180));
  } else {
    chassis_.set_steering_percentage(0.0);
    chassis_.set_steering_percentage_cmd(0.0);
  }

  if (chassis_detail.has_light_horn_report_362()) {
    //   // 14 high_beam_signal
    //   if (chassis_detail.light_horn_report_362().highbeam_status() == 1) {
    //     chassis_.set_high_beam_signal(true);
    //   } else {
    //     chassis_.set_high_beam_signal(false);
    //   }
    //   // 15 low_beam_signal
    //   if (char16_t(chassis_detail.light_horn_report_362().headbeam_status())
    //   ==
    //       1) {
    //     chassis_.set_low_beam_signal(true);
    //   } else {
    //     chassis_.set_low_beam_signal(false);
    //   }
    //   // 16 left_turn_signal
    //   if (chassis_detail.light_horn_report_362().turn_left_light_status() ==
    //   1) {
    //     chassis_.set_left_turn_signal(true);
    //   } else {
    //     chassis_.set_left_turn_signal(false);
    //   }

    //   // 17 right_turn_signal
    //   if (chassis_detail.light_horn_report_362().turn_right_light_status() ==
    //   1) {
    //     chassis_.set_right_turn_signal(true);
    //   } else {
    //     chassis_.set_right_turn_signal(false);
    //   }

    //   // 18 horn
    //   if (chassis_detail.light_horn_report_362().horn_status() == 1) {
    //     chassis_.set_horn(true);
    //   } else {
    //     chassis_.set_horn(false);

    // 27 signal
    auto signal = chassis_.mutable_signal();
    signal->set_high_beam(
        chassis_detail.light_horn_report_362().highbeam_status() == 1);
    signal->set_low_beam(
        chassis_detail.light_horn_report_362().headbeam_status() == 1);
    signal->set_horn(chassis_detail.light_horn_report_362().horn_status() == 1);

    if (chassis_detail.light_horn_report_362().emergencystop_status() == 1) {
      signal->set_emergency_light(true);
      signal->set_turn_signal(
          apollo::common::VehicleSignal::TURN_HAZARD_WARNING);
    } else {
      if (chassis_detail.light_horn_report_362().turn_left_light_status() ==
              0 &&
          chassis_detail.light_horn_report_362().turn_right_light_status() ==
              0) {
        last_signal_count_++;
        if (last_signal_count_ > 50) {
          signal->set_turn_signal(apollo::common::VehicleSignal::TURN_NONE);
          last_signal_count_ = 50;
        } else {
          signal->set_turn_signal(last_signal_);
        }
      } else {
        last_signal_count_ = 0;
        if (chassis_detail.light_horn_report_362().turn_left_light_status() ==
                1 &&
            chassis_detail.light_horn_report_362().turn_right_light_status() ==
                1) {
          signal->set_turn_signal(
              apollo::common::VehicleSignal::TURN_HAZARD_WARNING);
        } else {
          if (chassis_detail.light_horn_report_362().turn_left_light_status() ==
              1) {
            signal->set_turn_signal(apollo::common::VehicleSignal::TURN_LEFT);
          } else if (chassis_detail.light_horn_report_362()
                         .turn_right_light_status() == 1) {
            signal->set_turn_signal(apollo::common::VehicleSignal::TURN_RIGHT);
          }
        }
        last_signal_ = signal->turn_signal();
      }
    }
  } else {
    chassis_.clear_signal();
  }

  // 30 wheel_speed
  if (chassis_detail.has_scu_frontwheelspeed_301()) {
    auto wheelspeed = chassis_.mutable_wheel_speed();
    wheelspeed->set_wheel_spd_fl(
        chassis_detail.scu_frontwheelspeed_301().wheelspeed_fl());
    wheelspeed->set_wheel_spd_fr(
        chassis_detail.scu_frontwheelspeed_301().wheelspeed_fr());
    wheelspeed->set_is_wheel_spd_fl_valid(
        chassis_detail.scu_frontwheelspeed_301().wheelspeed_fl_valid());
    wheelspeed->set_is_wheel_spd_fr_valid(
        chassis_detail.scu_frontwheelspeed_301().wheelspeed_fr_valid());

    wheelspeed->set_wheel_direction_fl(transWheelDirection(
        chassis_detail.scu_frontwheelspeed_301().wheelspeed_fl_direct()));
    wheelspeed->set_wheel_direction_fr(transWheelDirection(
        chassis_detail.scu_frontwheelspeed_301().wheelspeed_fr_direct()));
  }
  if (chassis_detail.has_scu_rearwheelspeed_302()) {
    auto wheelspeed = chassis_.mutable_wheel_speed();
    wheelspeed->set_wheel_spd_rl(
        chassis_detail.scu_rearwheelspeed_302().wheelspeed_rl());
    wheelspeed->set_wheel_spd_rr(
        chassis_detail.scu_rearwheelspeed_302().wheelspeed_rr());
    wheelspeed->set_is_wheel_spd_rl_valid(
        chassis_detail.scu_rearwheelspeed_302().wheelspeed_rl_valid());
    wheelspeed->set_is_wheel_spd_rr_valid(
        chassis_detail.scu_rearwheelspeed_302().wheelspeed_rr_valid());
    wheelspeed->set_wheel_direction_rl(transWheelDirection(
        chassis_detail.scu_rearwheelspeed_302().wheelspeed_rl_direct()));
    wheelspeed->set_wheel_direction_rr(transWheelDirection(
        chassis_detail.scu_rearwheelspeed_302().wheelspeed_rr_direct()));
  }

  // 22 error_code
  if (chassis_detail.has_vcu_vehicle_fault_response_201()) {
    if (chassis_detail.vcu_vehicle_fault_response_201()
            .vehicle_error_indicationsvcu() == 0) {
      set_chassis_error_code(Chassis::NO_ERROR);
    } else {
      set_chassis_error_code(Chassis::CHASSIS_ERROR);
    }
    if (chassis_detail.vcu_vehicle_fault_response_201()
            .automode_exit_reason_press_remote_control() ||
        chassis_detail.vcu_vehicle_fault_response_201()
            .automode_exit_reason_pdu_control()) {
      set_chassis_error_code(Chassis::MANUAL_INTERVENTION);
    }
  } else {
    set_chassis_error_code(Chassis::UNKNOWN_ERROR);
  }

  // 33 vehicle_id
  if (chassis_detail.has_vehicle_vin_501()) {
    if (chassis_detail.vehicle_vin_501().has_vin_ascii_1() &&
        chassis_detail.vehicle_vin_501().has_vin_ascii_2() &&
        chassis_detail.vehicle_vin_501().has_vin_ascii_3()) {
      std::string vin = chassis_detail.vehicle_vin_501().vin_ascii_1();
      vin += chassis_detail.vehicle_vin_501().vin_ascii_2();
      vin += chassis_detail.vehicle_vin_501().vin_ascii_3();
      chassis_.mutable_vehicle_id()->set_vin(vin);
    }
  }

  // 12 add checkresponse signal
  if (chassis_detail.has_vcu_brake_report_47() &&
      chassis_detail.vcu_brake_report_47().has_brake_enable_resp()) {
    chassis_.mutable_check_response()->set_is_esp_online(
        chassis_detail.vcu_brake_report_47().brake_enable_resp() == 1);
  }
  if (chassis_detail.has_vcu_drive_report_52() &&
      chassis_detail.vcu_drive_report_52().has_drive_enable_resp()) {
    chassis_.mutable_check_response()->set_is_vcu_online(
        chassis_detail.vcu_drive_report_52().drive_enable_resp() == 1);
  }
  if (chassis_detail.has_vcu_eps_report_57() &&
      chassis_detail.vcu_eps_report_57().has_drive_enable_resp()) {
    chassis_.mutable_check_response()->set_is_eps_online(
        chassis_detail.vcu_eps_report_57().drive_enable_resp() == 1);
  }
  
  if (CheckChassisError()) {
    chassis_.mutable_engage_advice()->set_advice(
        apollo::common::EngageAdvice::DISALLOW_ENGAGE);
    chassis_.mutable_engage_advice()->set_reason(
        "Chassis has some fault, please check the chassis_detail.");
  }

  if (!chassis_error_mask_ && !chassis_.parking_brake() &&
      (chassis_.throttle_percentage() == 0.0)) {
    chassis_.mutable_engage_advice()->set_advice(
        apollo::common::EngageAdvice::READY_TO_ENGAGE);
  } else {
    chassis_.mutable_engage_advice()->set_advice(
        apollo::common::EngageAdvice::DISALLOW_ENGAGE);
    AERROR << "Disallow engage because chassis error mask is " << chassis_error_mask_
          << ", parking brake is " << chassis_.parking_brake()
          << ", throttle is " << chassis_.throttle_percentage();
  }

  return chassis_;
}

apollo::canbus::WheelSpeed_WheelSpeedType X3eproController::transWheelDirection(
    apollo::canbus::Wheelspeed_directType wheel_direction) {
  switch (wheel_direction) {
    case Wheelspeed_directType::WHEELSPEED_DIRECT_D:
      return apollo::canbus::WheelSpeed_WheelSpeedType_FORWARD;
      break;
    case Wheelspeed_directType::WHEELSPEED_DIRECT_R:
      return apollo::canbus::WheelSpeed_WheelSpeedType_BACKWARD;
      break;
    case Wheelspeed_directType::WHEELSPEED_DIRECT_N:
      return apollo::canbus::WheelSpeed_WheelSpeedType_STANDSTILL;
      break;
    default:
      break;
  }
  return apollo::canbus::WheelSpeed_WheelSpeedType_INVALID;
}

void X3eproController::Emergency() {
  set_driving_mode(Chassis::EMERGENCY_MODE);
  ResetProtocol();
}

ErrorCode X3eproController::EnableAutoMode() {
  if (driving_mode() == Chassis::COMPLETE_AUTO_DRIVE) {
    AINFO << "already in COMPLETE_AUTO_DRIVE mode";
    return ErrorCode::OK;
  }
  // set enable
  ads_brake_command_46_->set_drive_enable(
      Ads_brake_command_46::DRIVE_ENABLE_VALID);
  ads_drive_command_50_->set_drive_enable(
      Ads_drive_command_50::DRIVE_ENABLE_VALID);
  ads_eps_command_56_->set_drive_enable(Ads_eps_command_56::DRIVE_ENABLE_VALID);

  can_sender_->Update();
  const int32_t flag =
      CHECK_RESPONSE_STEER_UNIT_FLAG | CHECK_RESPONSE_SPEED_UNIT_FLAG;
  if (!CheckResponse(flag, true)) {
    AERROR << "Failed to switch to COMPLETE_AUTO_DRIVE mode. Please check the "
              "emergency button or chassis.";
    Emergency();
    set_chassis_error_code(Chassis::CHASSIS_ERROR);
    return ErrorCode::CANBUS_ERROR;
  }
  set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE);
  AINFO << "Switch to COMPLETE_AUTO_DRIVE mode ok.";
  return ErrorCode::OK;
}

ErrorCode X3eproController::DisableAutoMode() {
  ResetProtocol();
  can_sender_->Update();
  set_driving_mode(Chassis::COMPLETE_MANUAL);
  set_chassis_error_code(Chassis::NO_ERROR);
  AINFO << "Switch to COMPLETE_MANUAL ok.";
  return ErrorCode::OK;
}

ErrorCode X3eproController::EnableSteeringOnlyMode() {
  if (driving_mode() == Chassis::COMPLETE_AUTO_DRIVE ||
      driving_mode() == Chassis::AUTO_STEER_ONLY) {
    set_driving_mode(Chassis::AUTO_STEER_ONLY);
    AINFO << "Already in AUTO_STEER_ONLY mode.";
    return ErrorCode::OK;
  }
  // set enable
  ads_brake_command_46_->set_drive_enable(
      Ads_brake_command_46::DRIVE_ENABLE_INVALID);
  ads_drive_command_50_->set_drive_enable(
      Ads_drive_command_50::DRIVE_ENABLE_INVALID);
  ads_eps_command_56_->set_drive_enable(Ads_eps_command_56::DRIVE_ENABLE_VALID);

  can_sender_->Update();
  if (!CheckResponse(CHECK_RESPONSE_STEER_UNIT_FLAG, true)) {
    AERROR << "Failed to switch to AUTO_STEER_ONLY mode.";
    Emergency();
    set_chassis_error_code(Chassis::CHASSIS_ERROR);
    return ErrorCode::CANBUS_ERROR;
  }
  set_driving_mode(Chassis::AUTO_STEER_ONLY);
  AINFO << "Switch to AUTO_STEER_ONLY mode ok.";

  return ErrorCode::OK;
}

ErrorCode X3eproController::EnableSpeedOnlyMode() {
  if (driving_mode() == Chassis::COMPLETE_AUTO_DRIVE ||
      driving_mode() == Chassis::AUTO_SPEED_ONLY) {
    set_driving_mode(Chassis::AUTO_SPEED_ONLY);
    AINFO << "Already in AUTO_SPEED_ONLY mode";
    return ErrorCode::OK;
  }
  // set enable
  ads_brake_command_46_->set_drive_enable(
      Ads_brake_command_46::DRIVE_ENABLE_VALID);
  ads_drive_command_50_->set_drive_enable(
      Ads_drive_command_50::DRIVE_ENABLE_VALID);
  ads_eps_command_56_->set_drive_enable(
      Ads_eps_command_56::DRIVE_ENABLE_INVALID);

  can_sender_->Update();
  if (!CheckResponse(CHECK_RESPONSE_SPEED_UNIT_FLAG, true)) {
    AERROR << "Failed to switch to AUTO_SPEED_ONLY mode.";
    Emergency();
    set_chassis_error_code(Chassis::CHASSIS_ERROR);
    return ErrorCode::CANBUS_ERROR;
  }
  set_driving_mode(Chassis::AUTO_SPEED_ONLY);
  AINFO << "Switch to AUTO_SPEED_ONLY mode ok.";

  return ErrorCode::OK;
}

// NEUTRAL, REVERSE, DRIVE
void X3eproController::Gear(Chassis::GearPosition gear_position) {
  if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&
      driving_mode() != Chassis::AUTO_SPEED_ONLY) {
    AINFO << "This drive mode no need to set gear.";
    return;
  }

  switch (gear_position) {
    case Chassis::GEAR_NEUTRAL: {
      ads_drive_command_50_->set_auto_shift_command(
          Ads_drive_command_50::AUTO_SHIFT_COMMAND_N);
      ads_brake_command_46_->set_auto_parking_command(
          Ads_brake_command_46::AUTO_PARKING_COMMAND_RELEASE);
      break;
    }
    case Chassis::GEAR_REVERSE: {
      ads_drive_command_50_->set_auto_shift_command(
          Ads_drive_command_50::AUTO_SHIFT_COMMAND_R);
      break;
    }
    case Chassis::GEAR_DRIVE: {
      ads_drive_command_50_->set_auto_shift_command(
          Ads_drive_command_50::AUTO_SHIFT_COMMAND_D);
      break;
    }
    case Chassis::GEAR_PARKING: {
      ads_brake_command_46_->set_auto_parking_command(
          Ads_brake_command_46::AUTO_PARKING_COMMAND_APPLY);
      break;
    }
    default:
      break;
  }
}

// brake with pedal
// pedal:0.00~99.99, unit:percentage
void X3eproController::Brake(double pedal) {
  // double real_value = vehicle_params_.max_acceleration() * acceleration /
  // 100;
  // TODO(All) :  Update brake value based on mode
  if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&
      driving_mode() != Chassis::AUTO_SPEED_ONLY) {
    AINFO << "The current drive mode does not need to set brake pedal.";
    return;
  }
  ads_brake_command_46_->set_auto_brake_command(pedal);
}

// drive with pedal
// pedal:0.0~99.9 unit:percentage
void X3eproController::Throttle(double pedal) {
  if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&
      driving_mode() != Chassis::AUTO_SPEED_ONLY) {
    AINFO << "The current drive mode does not need to set throttle pedal.";
    return;
  }
  ads_drive_command_50_->set_auto_drive_torque(pedal / 100 * 64);
}

// confirm the car is driven by acceleration command instead of
// throttle/brake pedal drive with acceleration/deceleration acc:-7.0 ~ 5.0,
// unit:m/s^2
void X3eproController::Acceleration(double acc) {
  // None
}

// x3epro default, +500 ~ -500 or other, left:+, right:-
// need to be compatible with control module, so reverse
// steering with steering angle
// angle:99.99~0.00~-99.99, unit:deg, left:+, right:-
void X3eproController::Steer(double angle) {
  if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&
      driving_mode() != Chassis::AUTO_STEER_ONLY) {
    AINFO << "The current driving mode does not need to set steer.";
    return;
  }
  const double real_angle =
      vehicle_params_.max_steer_angle() / M_PI * 180 * angle / 100.0;
  ads_eps_command_56_->set_auto_target_angle(-real_angle);
}

// x3epro default, steering with new angle speed
// angle:99.99~0.00~-99.99, unit:deg, left:+, right:-
// angle_spd:0.00~99.99, unit:deg/s
void X3eproController::Steer(double angle, double angle_spd) {
  if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&
      driving_mode() != Chassis::AUTO_STEER_ONLY) {
    AINFO << "The current driving mode does not need to set steer.";
    return;
  }
  const double real_angle =
      vehicle_params_.max_steer_angle() / M_PI * 180 * angle / 100.0;
  ads_eps_command_56_->set_auto_target_angle(-real_angle);
}

void X3eproController::SetEpbBreak(const ControlCommand& command) {
  if (command.parking_brake()) {
    ads_brake_command_46_->set_auto_parking_command(
        Ads_brake_command_46::AUTO_PARKING_COMMAND_APPLY);
  } else {
    ads_brake_command_46_->set_auto_parking_command(
        Ads_brake_command_46::AUTO_PARKING_COMMAND_RELEASE);
  }
}

void X3eproController::SetBeam(const VehicleSignal& vehicle_signal) {
  if (vehicle_signal.has_low_beam()) {
    ads_lampcommand_309_->set_headbeam_command(
        vehicle_signal.low_beam() ? Ads_lampcommand_309::HEADBEAM_COMMAND_ON
                                  : Ads_lampcommand_309::HEADBEAM_COMMAND_OFF);
  }
}

void X3eproController::SetHorn(const VehicleSignal& vehicle_signal) {
  if (vehicle_signal.has_horn()) {
    ads_lampcommand_309_->set_horn_command(
        vehicle_signal.horn() ? Ads_lampcommand_309::HORN_COMMAND_ENABLE
                              : Ads_lampcommand_309::HORN_COMMAND_DISABLE);
  }
}

void X3eproController::SetTurningSignal(const VehicleSignal& vehicle_signal) {
  // Set Turn Signal
  if (vehicle_signal.has_turn_signal()) {
    switch (vehicle_signal.turn_signal()) {
      case VehicleSignal::TURN_LEFT:
        ads_lampcommand_309_->set_turn_left_light_command(
            Ads_lampcommand_309::TURN_LEFT_LIGHT_COMMAND_ENABLE);
        ads_lampcommand_309_->set_turn_right_light_command(
            Ads_lampcommand_309::TURN_RIGHT_LIGHT_COMMAND_DISABLE);
        break;
      case VehicleSignal::TURN_RIGHT:
        ads_lampcommand_309_->set_turn_left_light_command(
            Ads_lampcommand_309::TURN_LEFT_LIGHT_COMMAND_DISABLE);
        ads_lampcommand_309_->set_turn_right_light_command(
            Ads_lampcommand_309::TURN_RIGHT_LIGHT_COMMAND_ENABLE);
        break;
      case VehicleSignal::TURN_HAZARD_WARNING:
        ads_lampcommand_309_->set_turn_left_light_command(
            Ads_lampcommand_309::TURN_LEFT_LIGHT_COMMAND_ENABLE);
        ads_lampcommand_309_->set_turn_right_light_command(
            Ads_lampcommand_309::TURN_RIGHT_LIGHT_COMMAND_ENABLE);
        break;
      default:
        ads_lampcommand_309_->set_turn_left_light_command(
            Ads_lampcommand_309::TURN_LEFT_LIGHT_COMMAND_DISABLE);
        ads_lampcommand_309_->set_turn_right_light_command(
            Ads_lampcommand_309::TURN_RIGHT_LIGHT_COMMAND_DISABLE);
        break;
    }
  }
}

ErrorCode X3eproController::HandleCustomOperation(
    const external_command::ChassisCommand& command) {
  return ErrorCode::OK;
}

bool X3eproController::VerifyID() {
  if (!CheckVin()) {
    AERROR << "Failed to get the vin.";
    // GetVin();
    return false;
  } else {
    // ResetVin();
    return true;
  }
}

bool X3eproController::CheckVin() {
  if (chassis_.has_vehicle_id() && chassis_.vehicle_id().has_vin() &&
      chassis_.vehicle_id().vin().size() == 17) {
    AINFO << "Vin check success! Vehicel vin is "
          << chassis_.vehicle_id().vin();
    return true;
  }

  return false;
}

void X3eproController::GetVin() {}

void X3eproController::ResetVin() {}

void X3eproController::ResetProtocol() {
  message_manager_->ResetSendMessages();
}

bool X3eproController::CheckChassisError() {
  X3epro chassis_detail;
  if (message_manager_->GetSensorData(&chassis_detail) != ErrorCode::OK) {
    AERROR_EVERY(100) << "Get chassis detail failed.";
  }
  if (!chassis_.has_check_response()) {
    AERROR_EVERY(100) << "ChassisDetail has no x3epro vehicle info.";
    chassis_.mutable_engage_advice()->set_advice(
        apollo::common::EngageAdvice::DISALLOW_ENGAGE);
    chassis_.mutable_engage_advice()->set_reason(
        "ChassisDetail has no x3epro vehicle info.");
    return false;
  } else {
    chassis_.clear_engage_advice();
  }

  /* ADD YOUR OWN CAR CHASSIS OPERATION
  // steer fault
  // drive fault
  // brake fault
  */
  int8_t chassis_error_mask = 0;
  chassis_.mutable_engage_advice()->set_reason("");
  if (chassis_detail.has_vcu_vehicle_fault_response_201()) {
    // brake fault
    if (chassis_detail.vcu_vehicle_fault_response_201()
            .brake_system_errorehb() != 0) {
      AERROR_EVERY(100) << "ChassisDetail has Brake error. level "
                        << chassis_detail.vcu_vehicle_fault_response_201()
                               .brake_system_errorehb();
      chassis_.mutable_engage_advice()->set_advice(
          apollo::common::EngageAdvice::DISALLOW_ENGAGE);
      chassis_.mutable_engage_advice()->set_reason(
          chassis_.engage_advice().reason() + "Brake system error: level " +
          std::to_string(chassis_detail.vcu_vehicle_fault_response_201()
                             .brake_system_errorehb()) +
          "\n");
      chassis_error_mask |= 1;
    }
    // steer fault
    if (chassis_detail.vcu_vehicle_fault_response_201().eps_error() != 0) {
      AERROR_EVERY(100)
          << "ChassisDetail has Steer error. level "
          << chassis_detail.vcu_vehicle_fault_response_201().eps_error();
      chassis_.mutable_engage_advice()->set_advice(
          apollo::common::EngageAdvice::DISALLOW_ENGAGE);
      chassis_.mutable_engage_advice()->set_reason(
          chassis_.engage_advice().reason() + "EPS error: level " +
          std::to_string(
              chassis_detail.vcu_vehicle_fault_response_201().eps_error()) +
          "\n");
      chassis_error_mask |= 1;
    }
    // drive fault
    if (chassis_detail.vcu_vehicle_fault_response_201().motor_error() != 0) {
      AERROR_EVERY(100)
          << "ChassisDetail has Motor error. level "
          << chassis_detail.vcu_vehicle_fault_response_201().motor_error();
      chassis_.mutable_engage_advice()->set_advice(
          apollo::common::EngageAdvice::DISALLOW_ENGAGE);
      chassis_.mutable_engage_advice()->set_reason(
          chassis_.engage_advice().reason() + "Motor error: level " +
          std::to_string(
              chassis_detail.vcu_vehicle_fault_response_201().motor_error()) +
          "\n");
      chassis_error_mask |= 1;
    }
    // epb fault
    if (chassis_detail.vcu_vehicle_fault_response_201().epb_error() != 0) {
      AERROR_EVERY(100)
          << "ChassisDetail has EPB error. level "
          << chassis_detail.vcu_vehicle_fault_response_201().epb_error();
      chassis_.mutable_engage_advice()->set_advice(
          apollo::common::EngageAdvice::DISALLOW_ENGAGE);
      chassis_.mutable_engage_advice()->set_reason(
          chassis_.engage_advice().reason() + "EPB error: level " +
          std::to_string(
              chassis_detail.vcu_vehicle_fault_response_201().epb_error()) +
          "\n");
      chassis_error_mask |= 1;
    }
    // high voltage battery fault
    if (chassis_detail.vcu_vehicle_fault_response_201()
            .high_voltage_battery_errorbcu() != 0) {
      AERROR_EVERY(100) << "ChassisDetail has BCU error. level "
                        << chassis_detail.vcu_vehicle_fault_response_201()
                               .high_voltage_battery_errorbcu();
      chassis_.mutable_engage_advice()->set_advice(
          apollo::common::EngageAdvice::DISALLOW_ENGAGE);
      chassis_.mutable_engage_advice()->set_reason(
          chassis_.engage_advice().reason() +
          "high voltage battery error: level " +
          std::to_string(chassis_detail.vcu_vehicle_fault_response_201()
                             .high_voltage_battery_errorbcu()) +
          "\n");
      chassis_error_mask |= 1;
    }
  } else {
    AERROR_EVERY(100) << "ChassisDetail has no self checking response.";
    chassis_.mutable_engage_advice()->set_advice(
        apollo::common::EngageAdvice::DISALLOW_ENGAGE);
    chassis_.mutable_engage_advice()->set_reason(
        "ChassisDetail has no self checking info.");
    return false;
  }
  if (chassis_error_mask) {
    return true;
  }
  return false;
}

void X3eproController::SecurityDogThreadFunc() {
  int32_t vertical_ctrl_fail = 0;
  int32_t horizontal_ctrl_fail = 0;

  if (can_sender_ == nullptr) {
    AERROR << "Failed to run SecurityDogThreadFunc() because can_sender_ is "
              "nullptr.";
    return;
  }
  while (!can_sender_->IsRunning()) {
    std::this_thread::yield();
  }

  std::chrono::duration<double, std::micro> default_period{50000};
  int64_t start = 0;
  int64_t end = 0;
  while (can_sender_->IsRunning()) {
    start = ::apollo::cyber::Time::Now().ToMicrosecond();
    const Chassis::DrivingMode mode = driving_mode();
    bool emergency_mode = false;

    // 1. horizontal control check
    if ((mode == Chassis::COMPLETE_AUTO_DRIVE ||
         mode == Chassis::AUTO_STEER_ONLY) &&
        !CheckResponse(CHECK_RESPONSE_STEER_UNIT_FLAG, false)) {
      ++horizontal_ctrl_fail;
      if (horizontal_ctrl_fail >= kMaxFailAttempt) {
        emergency_mode = true;
        AERROR << "Driving_mode is into emergency by steer manual intervention";
        set_chassis_error_code(Chassis::MANUAL_INTERVENTION);
      }
    } else {
      horizontal_ctrl_fail = 0;
    }

    // 2. vertical control check
    if ((mode == Chassis::COMPLETE_AUTO_DRIVE ||
         mode == Chassis::AUTO_SPEED_ONLY) &&
        !CheckResponse(CHECK_RESPONSE_SPEED_UNIT_FLAG, false)) {
      ++vertical_ctrl_fail;
      if (vertical_ctrl_fail >= kMaxFailAttempt) {
        emergency_mode = true;
        AERROR << "Driving_mode is into emergency by speed manual intervention";
        set_chassis_error_code(Chassis::MANUAL_INTERVENTION);
      }
    } else {
      vertical_ctrl_fail = 0;
    }

    // 3. chassis fault check
    if (CheckChassisError()) {
      set_chassis_error_code(Chassis::CHASSIS_ERROR);
      emergency_mode = true;
    }

    if (emergency_mode && mode != Chassis::EMERGENCY_MODE) {
      set_driving_mode(Chassis::EMERGENCY_MODE);
      message_manager_->ResetSendMessages();
      can_sender_->Update();
    }
    end = ::apollo::cyber::Time::Now().ToMicrosecond();
    std::chrono::duration<double, std::micro> elapsed{end - start};
    if (elapsed < default_period) {
      std::this_thread::sleep_for(default_period - elapsed);
    } else {
      AERROR << "Too much time consumption in X3eproController looping process:"
             << elapsed.count();
    }
  }
}

bool X3eproController::CheckResponse(const int32_t flags, bool need_wait) {
  int32_t retry_num = 20;
  bool is_eps_online = false;
  bool is_vcu_online = false;
  bool is_esp_online = false;

  do {
    bool check_ok = true;
    if (flags & CHECK_RESPONSE_STEER_UNIT_FLAG) {
      is_eps_online = chassis_.has_check_response() &&
                      chassis_.check_response().has_is_eps_online() &&
                      chassis_.check_response().is_eps_online();
      check_ok = check_ok && is_eps_online;
    }

    if (flags & CHECK_RESPONSE_SPEED_UNIT_FLAG) {
      is_vcu_online = chassis_.has_check_response() &&
                      chassis_.check_response().has_is_vcu_online() &&
                      chassis_.check_response().is_vcu_online();
      is_esp_online = chassis_.has_check_response() &&
                      chassis_.check_response().has_is_esp_online() &&
                      chassis_.check_response().is_esp_online();
      check_ok = check_ok && is_vcu_online && is_esp_online;
    }
    if (check_ok) {
      return true;
    } else {
      AINFO << "Need to check response again.";
    }
    if (need_wait) {
      --retry_num;
      std::this_thread::sleep_for(
          std::chrono::duration<double, std::milli>(20));
    }
  } while (need_wait && retry_num);

  AERROR << "check_response fail: is_eps_online:" << is_eps_online
         << ", is_vcu_online:" << is_vcu_online
         << ", is_esp_online:" << is_esp_online;

  return false;
}

void X3eproController::set_chassis_error_mask(const int32_t mask) {
  std::lock_guard<std::mutex> lock(chassis_mask_mutex_);
  chassis_error_mask_ = mask;
}

int32_t X3eproController::chassis_error_mask() {
  std::lock_guard<std::mutex> lock(chassis_mask_mutex_);
  return chassis_error_mask_;
}

Chassis::ErrorCode X3eproController::chassis_error_code() {
  std::lock_guard<std::mutex> lock(chassis_error_code_mutex_);
  return chassis_error_code_;
}

void X3eproController::set_chassis_error_code(
    const Chassis::ErrorCode& error_code) {
  std::lock_guard<std::mutex> lock(chassis_error_code_mutex_);
  chassis_error_code_ = error_code;
}

}  // namespace x3epro
}  // namespace canbus
}  // namespace apollo
